#include "Encoder.h"


/*
  Encoder(int encA, int encB , int cpr, int index)
  - encA, encB    - encoder A and B pins
  - cpr           - counts per rotation number (cpm=ppm*4)
  - index pin     - (optional input)
*/

Encoder::Encoder(int _encA, int _encB, float _ppr, int _index)
{

    // Encoder measurement structure init
    // hardware pins
    pinA = _encA;
    pinB = _encB;
    // counter setup
    pulse_counter = 0;
    pulse_timestamp = 0;

    cpr = _ppr;
    A_active = 0;
    B_active = 0;
    I_active = 0;
    // index pin
    index_pin = _index; // its 0 if not used

    // velocity calculation variables
    prev_Th = 0;
    pulse_per_second = 0;
    prev_pulse_counter = 0;
    prev_timestamp_us = _micros();

    // extern pullup as default
    pullup = Pullup::USE_EXTERN;
    // enable quadrature encoder by default
    quadrature = Quadrature::ON;
}

//  Encoder interrupt callback functions
// A channel
void Encoder::handleA()
{
    bool A = digitalRead(pinA);
    switch (quadrature)
    {
        case Quadrature::ON:
            // CPR = 4xPPR
            if (A != A_active)
            {
                pulse_counter += (A_active == B_active) ? 1 : -1;
                pulse_timestamp = _micros();
                A_active = A;
            }
            break;
        case Quadrature::OFF:
            // CPR = PPR
            if (A && !digitalRead(pinB))
            {
                pulse_counter++;
                pulse_timestamp = _micros();
            }
            break;
    }
}

// B channel
void Encoder::handleB()
{
    bool B = digitalRead(pinB);
    switch (quadrature)
    {
        case Quadrature::ON:
            //     // CPR = 4xPPR
            if (B != B_active)
            {
                pulse_counter += (A_active != B_active) ? 1 : -1;
                pulse_timestamp = _micros();
                B_active = B;
            }
            break;
        case Quadrature::OFF:
            // CPR = PPR
            if (B && !digitalRead(pinA))
            {
                pulse_counter--;
                pulse_timestamp = _micros();
            }
            break;
    }
}

// Index channel
void Encoder::handleIndex()
{
    if (hasIndex())
    {
        bool I = digitalRead(index_pin);
        if (I && !I_active)
        {
            index_found = true;
            // align encoder on each index
            long tmp = pulse_counter;
            // corrent the counter value
            pulse_counter = round((double) pulse_counter / (double) cpr) * cpr;
            // preserve relative speed
            prev_pulse_counter += pulse_counter - tmp;
        }
        I_active = I;
    }
}

/*
	Shaft angle calculation
*/
float Encoder::getAngle()
{
    return _2PI * (pulse_counter) / ((float) cpr);
}

/*
  Shaft velocity calculation
  function using mixed time and frequency measurement technique
*/
float Encoder::getVelocity()
{
    // timestamp
    long timestamp_us = _micros();
    // sampling time calculation
    float Ts = (timestamp_us - prev_timestamp_us) * 1e-6;
    // quick fix for strange cases (micros overflow)
    if (Ts <= 0 || Ts > 0.5) Ts = 1e-3;

    // time from last impulse
    float Th = (timestamp_us - pulse_timestamp) * 1e-6;
    long dN = pulse_counter - prev_pulse_counter;

    // Pulse per second calculation (Eq.3.)
    // dN - impulses received
    // Ts - sampling time - time in between function calls
    // Th - time from last impulse
    // Th_1 - time form last impulse of the previous call
    // only increment if some impulses received
    float dt = Ts + prev_Th - Th;
    pulse_per_second = (dN != 0 && dt > Ts / 2) ? dN / dt : pulse_per_second;

    // if more than 0.05 passed in between impulses
    if (Th > 0.1) pulse_per_second = 0;

    // velocity calculation
    float velocity = pulse_per_second / ((float) cpr) * (_2PI);

    // save variables for next pass
    prev_timestamp_us = timestamp_us;
    // save velocity calculation variables
    prev_Th = Th;
    prev_pulse_counter = pulse_counter;
    return velocity;
}

// getter for index pin
// return -1 if no index
int Encoder::needsSearch()
{
    return hasIndex() && !index_found;
}

// private function used to determine if encoder has index
int Encoder::hasIndex()
{
    return index_pin != 0;
}


// encoder initialisation of the hardware pins
// and calculation variables
void Encoder::init()
{

    // Encoder - check if pullup needed for your encoder
    if (pullup == Pullup::USE_INTERN)
    {
        pinMode(pinA, INPUT_PULLUP);
        pinMode(pinB, INPUT_PULLUP);
        if (hasIndex()) pinMode(index_pin, INPUT_PULLUP);
    } else
    {
        pinMode(pinA, INPUT);
        pinMode(pinB, INPUT);
        if (hasIndex()) pinMode(index_pin, INPUT);
    }

    // counter setup
    pulse_counter = 0;
    pulse_timestamp = _micros();
    // velocity calculation variables
    prev_Th = 0;
    pulse_per_second = 0;
    prev_pulse_counter = 0;
    prev_timestamp_us = _micros();

    // initial cpr = PPR
    // change it if the mode is quadrature
    if (quadrature == Quadrature::ON) cpr = 4 * cpr;

}

// function enabling hardware interrupts of the for the callback provided
// if callback is not provided then the interrupt is not enabled
void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)())
{
    // attach interrupt if functions provided
    switch (quadrature)
    {
        case Quadrature::ON:
            // A callback and B callback
            if (doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE);
            if (doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE);
            break;
        case Quadrature::OFF:
            // A callback and B callback
            if (doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, RISING);
            if (doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, RISING);
            break;
    }

    // if index used initialize the index interrupt
    if (hasIndex() && doIndex != nullptr) attachInterrupt(digitalPinToInterrupt(index_pin), doIndex, CHANGE);
}
